{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix\n",
    "init_printing(use_latex='mathjax')\n",
    "from scipy import weave\n",
    "import os\n",
    "import cv2\n",
    "import pickle\n",
    "from robot import transl,r2t,t2r,rpy2tr"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 将z轴转动到一个向量,带权值"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "x = mat([1,0,0]).reshape((3,1))\n",
    "y = mat([0,1,0]).reshape((3,1))\n",
    "z = mat([0,0,1]).reshape((3,1))\n",
    "# 各个轴的叉积关系\n",
    "assert np.allclose(np.cross(z,x,axis=0),y)\n",
    "assert np.allclose(np.cross(y,z,axis=0),x)\n",
    "assert np.allclose(np.cross(x,y,axis=0),z)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def unit(w):\n",
    "    w = w/norm(w)\n",
    "    return w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_angle(vec1,vec2):\n",
    "    dot = (vec1.T*vec2)[0,0]\n",
    "    return np.arccos(dot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_proj_xy(vec):\n",
    "    # 将单位向量 x,y 投影到垂直于向量 vec 的平面\n",
    "    # 得到新单位向量 proj_x, proj_y\n",
    "    vec = unit(vec)\n",
    "    new_z = vec[:3,0]\n",
    "    a,b,c = array(vec[:3,0]).flatten()\n",
    "    proj_x = np.mat([1-a**2, - a*b, - a*c]).reshape((3,1))\n",
    "    proj_x = proj_x/np.linalg.norm(proj_x) # 单位化\n",
    "    # 原 y 轴单位向量在与 new_z 垂直平面的投影向量\n",
    "    proj_y = np.mat([-a*b,1-b**2,-b*c]).reshape((3,1))\n",
    "    proj_y = proj_y/np.linalg.norm(proj_y) # 单位化\n",
    "    return proj_x,proj_y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 实验证明,投影后的单位矩阵角度可能小于 90 度,也可能大于 90 度,等于90度\n",
    "def check_jiajiao():\n",
    "    for x in np.arange(-1,1,0.1):\n",
    "        for y in np.arange(-1,1,0.1):\n",
    "            for z in np.arange(0,1,0.1):\n",
    "                vec = mat([x,y,z]).reshape((3,1))\n",
    "                proj_x,proj_y = get_proj_xy(vec)\n",
    "                print vec\n",
    "                print rad2deg(get_angle(proj_x,proj_y))\n",
    "#check_jiajiao()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_new_xy(vec):\n",
    "    weight_x = 1.0 # 权重越大转动量越小\n",
    "    weight_y = 1.0\n",
    "    new_z = unit(vec[:3,0])\n",
    "    proj_x,proj_y = get_proj_xy(new_z)\n",
    "    theta0 = get_angle(proj_x,proj_y)\n",
    "    if abs(theta0 - pi/2) < 1e-3:\n",
    "        return proj_x,proj_y\n",
    "    else:\n",
    "        # x轴投影向量作为新的 x 轴向量则与原 x 轴的夹角最小\n",
    "        # 综合考虑,将新的x轴向量绕 新的z轴转动一个角度\n",
    "        theta1 = (theta0 - np.pi/2)/(1+weight_x/weight_y)\n",
    "        new_x = np.mat(cv2.Rodrigues(vec*theta1)[0])*proj_x\n",
    "        new_y = np.cross(new_z,new_x,axis=0)\n",
    "    print \"theta1:\", theta1\n",
    "    print \"<new_x,proj_x>:\",get_angle(new_x,proj_x)\n",
    "    print \"<new_y,proj_y>:\",get_angle(new_y,proj_y)\n",
    "    return new_x,new_y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def plot_vec(vec,c):\n",
    "    x,y = array(vec[:2,0]).flatten()\n",
    "    plot([0,x],[0,y],c)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def plot_xy(vec):\n",
    "    # 从 vec 看去x,y的图像\n",
    "    ax = plt.gca()\n",
    "    ax.set_aspect(1) # 固定缩放比\n",
    "    xlim([-0.2,1.0])\n",
    "    ylim([-0.2,1.0])\n",
    "    new_z = unit(vec[:3,0])\n",
    "    new_x,new_y = get_new_xy(new_z)\n",
    "    R_new_0 = mat(np.hstack((new_x,new_y,new_z))).T\n",
    "    x = mat([1,0,0]).reshape((3,1))\n",
    "    y = mat([0,1,0]).reshape((3,1))\n",
    "    plot_x = R_new_0*x\n",
    "    plot_vec(plot_x,'--r')\n",
    "    plot_y = R_new_0*y\n",
    "    plot_vec(plot_y,'--b')\n",
    "    plot_new_x = unit(R_new_0*new_x)\n",
    "    plot_vec(plot_new_x,'r')\n",
    "    plot_new_y = unit(R_new_0*new_y)\n",
    "    plot_vec(plot_new_y,'b')\n",
    "    #print \"new_x:\",new_x\n",
    "    #print \"plot_new_x:\",plot_new_x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 投影夹角为钝角\n",
    "vec = np.mat([0.5,0.8,1.0]).reshape((3,1)) # 指定向量\n",
    "vec = unit(vec)\n",
    "plot_xy(vec)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 投影夹角为锐角\n",
    "vec = np.mat([0.1,-0.9,0.6]).reshape((3,1)) # 指定向量\n",
    "vec = unit(vec)\n",
    "plot_xy(vec)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_R_nowobj_obj(vec,weight_x,weight_y):\n",
    "    new_z = vec[:3]\n",
    "    a,b,c = array(vec[:3,0]).flatten()\n",
    "    # 原 x 轴单位向量在与 new_z 垂直平面的投影向量\n",
    "    proj_x = np.mat([1-a**2, - a*b, - a*c]).reshape((3,1))\n",
    "    proj_x = proj_x/np.linalg.norm(proj_x) # 单位化\n",
    "    # 原 y 轴单位向量在与 new_z 垂直平面的投影向量\n",
    "    proj_y = np.mat([-a*b,1-b**2,-b*c]).reshape((3,1))\n",
    "    proj_y = proj_y/np.linalg.norm(proj_y) # 单位化\n",
    "    # 两个轴的夹角\n",
    "    dot = (proj_x.T*proj_y)[0,0]\n",
    "    theta0 = np.arccos(dot)\n",
    "    # x轴投影向量作为新的 x 轴向量则与原 x 轴的夹角最小\n",
    "    # 综合考虑,将新的x轴向量绕 新的z轴转动一个角度\n",
    "    theta1 = (theta0 - np.pi/2)/(1+weight_x/weight_y)\n",
    "    new_x = np.mat(cv2.Rodrigues(vec*theta1)[0])*proj_x\n",
    "    new_y = np.cross(new_z,new_x,axis=0)\n",
    "    R_newobj_obj = np.mat(np.hstack((new_x,new_y,new_z))).T\n",
    "    return R_newobj_obj"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "vec = np.mat([0.5,0.8,1.0]).reshape((3,1)) # 指定直线的方向向量, 在 obj 坐标系下的表示\n",
    "vec = unit(vec)\n",
    "vec"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "R_nowobj_obj = get_R_nowobj_obj(vec,1.0,1.0)\n",
    "R_nowobj_obj*vec # 在 obj 下的向量 vec 应该为 newobj 的z轴"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 保持其在z方向不变\n",
    "\n",
    "目标是得到新的目标位姿, Tmo_newobj"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_obj_pos(Tmo_obj,point):\n",
    "    x,y = array(point[:2]).flatten()\n",
    "    Tmo_lineobj = Tmo_obj*transl(x,y,0)\n",
    "    return Tmo_lineobj[:3,3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_obj_mz(Tmo_obj,point):\n",
    "    '''\n",
    "    获取指定obj的Oxy平面上一点在 master 机器人坐标系下的 z 值\n",
    "    :param Tmo_obj:  {}_{obj}^{mo} T 指定obj在 master 机器人坐标系下的位姿\n",
    "    :param point:  在指定 obj 下 Oxy 平面上一点的坐标 (x,y)\n",
    "    :return: mz\n",
    "    '''\n",
    "    x,y = array(point[:2]).flatten()\n",
    "    Tmo_lineobj = Tmo_obj*transl(x,y,0)\n",
    "    return Tmo_lineobj[2,3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def set_obj_mz(Tmo_obj,point,mz,vec):\n",
    "    '''\n",
    "    设定 指定obj的Oxy平面上一点在 master 机器人坐标系下的 z 值\n",
    "    :param Tmo_obj: 指定obj在 master 机器人坐标系下的位姿\n",
    "    :param point: 在指定 obj 下 Oxy 平面上一点的坐标 (x,y)\n",
    "    :param mz: 设定的 z 值\n",
    "    :param vec: 在 master 坐标系下用来平移的方向向量\n",
    "    :return: new_Tmo_obj 新的坐标系\n",
    "    '''\n",
    "    vec = unit(vec)\n",
    "    old_obj_mz = Tmo_obj[2,3]\n",
    "    old_mz = get_obj_mz(Tmo_obj,point)\n",
    "    delta = old_obj_mz - old_mz\n",
    "    obj_mz = delta + mz\n",
    "    obj_pos = Tmo_obj[:3,3]\n",
    "    k = (obj_mz - old_obj_mz)/vec[2,0]\n",
    "    new_obj_pos = obj_pos + k*vec\n",
    "    new_Tmo_ojb = Tmo_obj.copy()\n",
    "    new_Tmo_ojb[:3,3] = new_obj_pos\n",
    "    return new_Tmo_ojb"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tmo_obj = transl(1.2,2.3,1.4)*rpy2tr(0.2,0.1,0.3) # 初始目标位姿在 mo 坐标系下的表示\n",
    "Point = np.mat([1.0,2.0]).reshape((2,1)) # 初始定点\n",
    "vec = np.mat([0.5,0.8,1.0]).reshape((3,1)) # 指定直线的方向向量, 在 mo 下的表示\n",
    "vec = unit(vec)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "x = mat([1,0,0]).reshape((3,1))\n",
    "y = mat([0,1,0]).reshape((3,1))\n",
    "z = mat([0,0,1]).reshape((3,1))\n",
    "obj_x = t2r(Tmo_obj)*x\n",
    "obj_y = t2r(Tmo_obj)*y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 按照定义计算指定点 Point 在 master 坐标系下的三维坐标\n",
    "Tobj_pos = Tmo_obj[:3,3]\n",
    "Tmo_point_pos = Tobj_pos + Point[0,0]*obj_x + Point[1,0]*obj_y\n",
    "Tmo_point_pos"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 利用矩阵变换计算指定点 Point 在 master 坐标系下的三维坐标\n",
    "Tmo_lineobj = Tmo_obj*transl(Point[0,0],Point[1,0],0)\n",
    "np.allclose(Tmo_lineobj[:3,3],Tmo_point_pos) # 验证两者相同,即变换矩阵理解正确"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "get_obj_mz(Tmo_obj,Point) # 指定点 Point 在 master 坐标系下的 z 坐标"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "new_Tmo_obj = set_obj_mz(Tmo_obj,Point,2.1,vec)\n",
    "get_obj_mz(new_Tmo_obj,Point)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 实践,含平移和旋转"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tmo_obj = transl(1.2,2.3,1.4)*rpy2tr(0.2,0.1,0.3) # 初始目标位姿在 mo 坐标系下的表示\n",
    "Point = np.mat([1.0,2.0]).reshape((2,1)) # 初始定点\n",
    "init_mz = get_obj_mz(Tmo_obj,Point)\n",
    "linePoint = np.mat([1.5,2.4]).reshape((2,1)) # 当前直线与平面的交点\n",
    "vec = np.mat([0.5,0.8,1.0]).reshape((3,1)) # 指定直线的方向向量, 在 obj 坐标系下的表示\n",
    "vec = unit(vec)\n",
    "R_newobj_obj = get_R_nowobj_obj(vec,1.0,1.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "x,y = array(Point).flatten()\n",
    "dx,dy = array(linePoint-Point).flatten()\n",
    "# 先将 {obj} 坐标系原点沿着 {obj} 坐标系移动到点 Point, 得到坐标系 {obj_point}\n",
    "# 再将 {obj_point} 变换到新的坐标系 {obj_line_point}\n",
    "# 再将 {obj_line_point} 反向移动, 得到 {new_obj_point}\n",
    "new_Tmo_new_obj = Tmo_obj*transl(x,y,0)*transl(dx,dy,0)*r2t(inv(R_newobj_obj))*transl(-x,-y,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 验算, 在新的坐标系下 Point 点在 master坐标系下坐标应为 在旧坐标系下 linePoint 点在 master坐标系下坐标\n",
    "get_obj_pos(new_Tmo_new_obj,Point)-get_obj_pos(Tmo_obj,linePoint)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 但新的坐标系的 mz 改变了\n",
    "get_obj_mz(new_Tmo_new_obj,Point) - init_mz"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "new_Tmo_new_obj = set_obj_mz(new_Tmo_new_obj,Point,init_mz,t2r(Tmo_obj)*vec)\n",
    "get_obj_mz(new_Tmo_new_obj,Point) - init_mz"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "验算:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 新坐标系末端 z 轴向量应该与在obj表示的 vec 平行\n",
    "z = mat([0,0,1]).reshape((3,1))\n",
    "new_z = t2r(new_Tmo_new_obj) * z\n",
    "new_z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "t2r(Tmo_obj)*vec"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
